#include "usr_inc.h"

#define BUF5_MAX_SIZE 16

/* 定义uart1接收的相关变量 */
UartRx_t rx1;
uint8_t buf1No1[32];
uint8_t buf1No2[32];
uint8_t buf1No3[32];
uint8_t buf1No4[32];
uint8_t buf1No5[32];
float lastRadarYaw = 0.0;
int16_t ovRadar = 0;

/* 定义uart4接收的相关变量 */
uint8_t buf4[15];
extern Locater_t loc;

/* 定义uart5接收体实例 */
UartRx_t rx5;
uint8_t buf5No1[BUF5_MAX_SIZE];
uint8_t buf5No2[BUF5_MAX_SIZE];
uint8_t buf5No3[BUF5_MAX_SIZE];
uint8_t buf5No4[BUF5_MAX_SIZE];

/**
 * @brief 初始化所有串口接收体实例
 */
void ALL_BSP_UART_Init(void)
{
    BSP_UART_Init(&rx1, &huart1);
    HAL_UART_Receive_IT(rx1.huart, &rx1.data, 1);

    HAL_UART_Transmit(&huart4, "A", sizeof("A"), 2);
    HAL_UART_Receive_DMA(&huart4, buf4, 15);

    BSP_UART_Init(&rx5, &huart5);
    HAL_UART_Receive_IT(rx5.huart, &rx5.data, 1);
}

/**
 * @brief 重定义串口接收回调函数
 */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if (huart == &huart1)
    {
        /*long time ago*/
        // if (rx1.state == 0 && rx1.data == '~')
        // {
        //     rx1.state = 1;
        // }
        // else if (rx1.state == 1 && rx1.data == '!')
        // {
        //     rx1.state = 2;
        // }
        // else if (rx1.state == 2)
        // {
        //     if (rx1.data == '@')
        //     {
        //         buf1No1[rx1.cnt] = '\0';
        //
        //         float x = atof(buf1No1);
        //         xQueueOverwriteFromISR(radarBoxHandle, &x, NULL);
        //
        //         rx1.cnt = 0;
        //         rx1.state = 0;
        //     }
        //     else
        //     {
        //         buf1No1[rx1.cnt++] = rx1.data;
        //     }
        // }
        // if (rx1.state == 0 && rx1.data == '#')
        // {
        //     rx1.state = 3;
        // }
        // else if (rx1.state == 3 && rx1.data == '$')
        // {
        //     rx1.state = 4;
        // }
        // else if (rx1.state == 4)
        // {
        //     if (rx1.data == '@')
        //     {
        //         buf1No2[rx1.cnt] = '\0';
        //
        //         float x = atof(buf1No2);
        //         xQueueOverwriteFromISR(radarDistanceBoxHandle, &x, NULL);
        //
        //         rx1.cnt = 0;
        //         rx1.state = 0;
        //     }
        //     else
        //     {
        //         buf1No2[rx1.cnt++] = rx1.data;
        //     }
        // }

        if (rx1.state == 0 && rx1.data == 'L')
        {
            rx1.state = 5;
        }
        else if (rx1.state == 5 && rx1.data == '=')
        {
            rx1.state = 6;
        }
        else if (rx1.state == 6)
        {
            if (rx1.data == ',')
            {
                buf1No3[rx1.cnt] = '\0';

                rx1.cnt = 0;
                rx1.state = 7;
            }
            else
            {
                buf1No3[rx1.cnt++] = rx1.data;
            }
        }
        else if (rx1.state == 7)
        {
            if (rx1.data == ',')
            {
                buf1No4[rx1.cnt] = '\0';

                rx1.cnt = 0;
                rx1.state = 8;
            }
            else
            {
                buf1No4[rx1.cnt++] = rx1.data;
            }
        }
        else if (rx1.state == 8)
        {
            if (rx1.data == '!')
            {
                buf1No5[rx1.cnt] = '\0';

                LocaterMsg_t msg;
                msg.posX = atof(buf1No3);
                msg.posY = atof(buf1No4);
                msg.posYaw = atof(buf1No5);

                float temp = msg.posYaw - lastRadarYaw;
                if (fabsf(temp) > M_PI)
                {
                    temp < 0.0 ? ovRadar++ : ovRadar--;
                }
                lastRadarYaw = msg.posYaw;

                msg.posYaw = msg.posYaw + ovRadar * 2.0 * M_PI;

                xQueueOverwriteFromISR(radarLocHandle, &msg, NULL);

                rx1.cnt = 0;
                rx1.state = 0;
            }
            else
            {
                buf1No5[rx1.cnt++] = rx1.data;
            }
        }

        /*another*/
        // if (rx1.state == 0 && rx1.data == 0x7E)
        // {
        //     rx1.state = 9;
        // }
        // else if (rx1.state == 9)
        // {
        //     if (rx1.cnt >= 12)
        //     {
        //         // if (initFlag == DISABLE)
        //         // {
        //         //     if (initCnt <= 15)
        //         //     {
        //         //         initX += (float)((int32_t)buf1No5[0] << 24 | (int32_t)buf1No5[1] << 16 | (int32_t)buf1No5[2] << 8 | (int32_t)buf1No5[3]);
        //         //         initY += (float)((int32_t)buf1No5[4] << 24 | (int32_t)buf1No5[5] << 16 | (int32_t)buf1No5[6] << 8 | (int32_t)buf1No5[7]);
        //         //         initYaw += (float)((int32_t)buf1No5[8] << 24 | (int32_t)buf1No5[9] << 16 | (int32_t)buf1No5[10] << 8 | (int32_t)buf1No5[11]);
        //         //
        //         //         initCnt++;
        //         //     }
        //         //     else
        //         //     {
        //         //         initX = initX / 16.0;
        //         //         initY = initY / 16.0;
        //         //         initYaw = initYaw / 16.0;
        //         //
        //         //         initFlag = ENABLE;
        //         //     }
        //         // }
        //         // else
        //         {
        //             // printf("1\r\n");
        //
        //             LocaterMsg_t msg;
        //             msg.posX = -(float)((int32_t)buf1No5[4] << 24 | (int32_t)buf1No5[5] << 16 | (int32_t)buf1No5[6] << 8 | (int32_t)buf1No5[7]);
        //             msg.posY = -(float)((int32_t)buf1No5[0] << 24 | (int32_t)buf1No5[1] << 16 | (int32_t)buf1No5[2] << 8 | (int32_t)buf1No5[3]);
        //             msg.posYaw = -(float)((int32_t)buf1No5[8] << 24 | (int32_t)buf1No5[9] << 16 | (int32_t)buf1No5[10] << 8 | (int32_t)buf1No5[11]);
        //
        //             msg.posX /= 10000.0;
        //             msg.posY /= 10000.0;
        //             msg.posYaw /= 10000.0;
        //
        //             xQueueOverwriteFromISR(radarLocHandle, &msg, NULL);
        //         }
        //
        //         rx1.cnt = 0;
        //         rx1.state = 0;
        //     }
        //     else
        //     {
        //         buf1No5[rx1.cnt++] = rx1.data;
        //     }
        // }

        /*chf*/
        // if (rx1.state == 0 && rx1.data == 'A')
        // {
        //     rx1.state = 10;
        // }
        // else if (rx1.state == 10)
        // {
        //     if (rx1.cnt >= 12)
        //     {
        //         LocaterMsg_t msg;
        //         msg.posY = (float)((int32_t)buf1No5[7] << 24 | (int32_t)buf1No5[6] << 16 | (int32_t)buf1No5[5] << 8 | (int32_t)buf1No5[4]);
        //         msg.posX = (float)((int32_t)buf1No5[3] << 24 | (int32_t)buf1No5[2] << 16 | (int32_t)buf1No5[1] << 8 | (int32_t)buf1No5[0]);
        //         msg.posYaw = (float)((int32_t)buf1No5[11] << 24 | (int32_t)buf1No5[10] << 16 | (int32_t)buf1No5[9] << 8 | (int32_t)buf1No5[8]);
        //
        //         msg.posX /= 1000.0;
        //         msg.posY /= 1000.0;
        //         msg.posYaw /= 1000.0;
        //
        //         float temp = msg.posYaw - lastRadarYaw;
        //         if (fabsf(temp) > M_PI)
        //         {
        //             temp < 0.0 ? ovRadar++ : ovRadar--;
        //         }
        //         lastRadarYaw = msg.posYaw;
        //
        //         msg.posYaw = msg.posYaw + ovRadar * 2.0 * M_PI;
        //
        //         if (fabsf(msg.posX) + fabsf(msg.posY) + fabsf(msg.posYaw) < 10000.0)
        //         {
        //             xQueueOverwriteFromISR(radarLocHandle, &msg, NULL);
        //         }
        //
        //         rx1.cnt = 0;
        //         rx1.state = 0;
        //     }
        //     else
        //     {
        //         buf1No5[rx1.cnt++] = rx1.data;
        //     }
        // }

        HAL_UART_Receive_IT(rx1.huart, &rx1.data, 1);
    }
    else if (huart == &huart4)
    {
        if (buf4[0] == 'P' && buf4[1] == 'G')
        {
            HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_RESET);

            const Locater_t last = loc;
            memcpy(&loc.posX, buf4 + 2, sizeof(float));
            memcpy(&loc.posY, buf4 + 6, sizeof(float));
            memcpy(&loc.rawYaw, buf4 + 10, sizeof(float));

            if (loc.rawYaw - last.rawYaw >= 180.0)
            {
                loc.ovCnt--;
            }
            else if (loc.rawYaw - last.rawYaw <= -180.0)
            {
                loc.ovCnt++;
            }

            LocaterMsg_t msg;
            msg.posX = loc.posX * 0.01;
            msg.posY = loc.posY * 0.01;
            msg.posYaw = loc.rawYaw + loc.ovCnt * 360.0;
            msg.velYaw = (loc.rawYaw - last.rawYaw + (loc.ovCnt - last.ovCnt) * 360.0) * 200.0 / 180.0 * M_PI;
            xQueueOverwriteFromISR(locaterMsgBoxHandle, &msg, NULL);
        }

        HAL_UART_Receive_DMA(&huart4, buf4, 15);
    }
    else if (huart == &huart5)
    {
        if (rx5.state == 0 && rx5.data == '@')
        {
            rx5.state = 1;
        }
        else if (rx5.state == 1 && rx5.data == '#')
        {
            rx5.state = 2;
        }
        else if (rx5.state == 2)
        {
            if (rx5.cnt >= BUF5_MAX_SIZE)
            {
                rx5.state = 0;
                rx5.cnt = 0;
            }

            if (rx5.data == '$')
            {
                buf5No1[rx5.cnt] = '\0';

                rx5.state = 3;
                rx5.cnt = 0;
            }
            else
            {
                buf5No1[(rx5.cnt)++] = rx5.data;
            }
        }
        else if (rx5.state == 3)
        {
            if (rx5.cnt >= BUF5_MAX_SIZE)
            {
                rx5.state = 0;
                rx5.cnt = 0;
            }

            if (rx5.data == '%')
            {
                buf5No2[rx5.cnt] = '\0';

                rx5.state = 4;
                rx5.cnt = 0;
            }
            else
            {
                buf5No2[(rx5.cnt)++] = rx5.data;
            }
        }
        else if (rx5.state == 4)
        {
            if (rx5.cnt >= BUF5_MAX_SIZE)
            {
                rx5.state = 0;
                rx5.cnt = 0;
            }

            if (rx5.data == '^')
            {
                buf5No3[rx5.cnt] = '\0';

                rx5.state = 5;
                rx5.cnt = 0;
            }
            else
            {
                buf5No3[(rx5.cnt)++] = rx5.data;
            }
        }
        else if (rx5.state == 5)
        {
            if (rx5.cnt >= BUF5_MAX_SIZE)
            {
                rx5.state = 0;
                rx5.cnt = 0;
            }

            if (rx5.data == '&')
            {
                buf5No4[rx5.cnt] = '\0';

                PidArgsMsg_t args = {0};
                args.target = atof(buf5No1);
                args.kp = atof(buf5No2);
                args.ki = atof(buf5No3);
                args.kd = atof(buf5No4);
                xQueueSendToBackFromISR(pidArgsQueueHandle, &args, NULL);

                rx5.state = 0;
                rx5.cnt = 0;
            }
            else
            {
                buf5No4[(rx5.cnt)++] = rx5.data;
            }
        }

        HAL_UART_Receive_IT(rx5.huart, &rx5.data, 1);
    }
}

/**
 * @brief 重定向printf到USART5
 */
#ifdef __GNUC__
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif
PUTCHAR_PROTOTYPE
{
    HAL_UART_Transmit(&huart5, (uint8_t *)&ch, 1, 0xFFFF);
    return ch;
}
